package utils;

import rasterprimate.Constants;
import rasterprimate.Primate;
import rasterprimate.PrimateTopo;
import rasterprimate.Resource;
import repast.simphony.random.RandomHelper;
import repast.simphony.space.grid.GridPoint;

import com.vividsolutions.jts.geom.Coordinate;

public final strictfp  class MoveUtils {

	//(1)
	public static void moveTo(Primate agent,Coordinate c){//used when primate has chosen a food site within it's search (move) radius

		//Calculate where to move inside patch
		double newX = c.x+RandomHelper.nextDoubleFromTo( (-Constants.cellSize/2.0), (Constants.cellSize/2.0));
		double newY = c.y+RandomHelper.nextDoubleFromTo( (-Constants.cellSize/2.0), (Constants.cellSize/2.0));
		Coordinate newCoord = new Coordinate(newX,newY);

		//calculate angle at which to move
		double adjasent = (double)(newCoord.x-agent.getCoord().x);
		double opposite = (double)(newCoord.y-agent.getCoord().y);
		double angle=0;

		angle = Math.atan(Math.abs(opposite/adjasent));
		if (adjasent<0) angle=(Math.PI)-angle;
		if (opposite<0) angle=2*(Math.PI)-angle;

		//calculate distance
		double moveDist = agent.getCoord().distance(newCoord);

		//check to make sure not moveing more than the max distance
		if (moveDist>Constants.maxDistPerStep)moveDist=Constants.maxDistPerStep;

		//Perform move; if distance is outsite current foodsite
		//geog.moveByVector(this, moveDist, (angle));
		agent.getGeog().moveByDisplacement(agent, Math.cos(angle)*moveDist, Math.sin(angle)*moveDist);
		Resource newRes = SimUtils.findMyCurrentResource(agent, agent.getGeog());
		//agent.getGrid().moveTo(agent,newRes.getGridx(),newRes.getGridy());

		agent.updateMyPatch(newRes);
		agent.addNumbMoves();

	}

	//(2)
	public static void moveToNearestGroup(Primate agent){//used if primate considers it self not safe
		Coordinate group = GroupUtils.myNearestNeighs(agent);
		//myGroupsCenter()
			if (group!=null){
				if (agent.getCoord().distance(group)>Constants.cellSize/2.0){
					moveTo(agent,group);//moves towards my nearest neighbours... number determined by desiered safe neigh size
				}
			}else{
				System.out.println("individual cannot find it's group: move to nearest Group method, within the MoveUtils class!");
				//Coordinate random = new Coordinate(this.getCoord().x+RandomHelper.nextDoubleFromTo(-1, 1),this.getCoord().y+RandomHelper.nextDoubleFromTo(-1, 1));
				//moveTo(random);
			}
	}
	
	public static void moveTowards (Primate p, Coordinate c){
		
		//calculate angle at which to move
		double adjasent = (double)(c.x-p.getCoord().x);
		double opposite = (double)(c.y-p.getCoord().y);
		double angle=0;

		angle = Math.atan(Math.abs(opposite/adjasent));
		if (adjasent<0) angle=(Math.PI)-angle;
		if (opposite<0) angle=2*(Math.PI)-angle;

		p.getGeog().moveByDisplacement(p, Math.cos(angle)*Constants.cellSize, Math.sin(angle)*Constants.cellSize);
		
	}
	
	public static void moveTowardsLeader(Primate p){
		//locate leader
		Coordinate c = null;
		for (Primate groupMate: p.getGroupMates()){
			if (groupMate.idNumb==0)c=groupMate.getCoord();
		}
		
		if (c == null){
			moveToNearestGroup(p);
		}else{
			moveTowards(p,c);
		}
		
	}
	
	/*************************************TOPO************************************************************/

	
	//(1)
	public static void moveTo(PrimateTopo agent,Coordinate c){//used when primate has chosen a food site within it's search (move) radius

		//Calculate where to move inside patch
		double newX = c.x+RandomHelper.nextDoubleFromTo( (-Constants.cellSize/2.0), (Constants.cellSize/2.0));
		double newY = c.y+RandomHelper.nextDoubleFromTo( (-Constants.cellSize/2.0), (Constants.cellSize/2.0));
		Coordinate newCoord = new Coordinate(newX,newY);

		//calculate angle at which to move
		double adjasent = (double)(newCoord.x-agent.getCoord().x);
		double opposite = (double)(newCoord.y-agent.getCoord().y);
		double angle=0;

		angle = Math.atan(Math.abs(opposite/adjasent));
		if (adjasent<0) angle=(Math.PI)-angle;
		if (opposite<0) angle=2*(Math.PI)-angle;

		//calculate distance
		double moveDist = agent.getCoord().distance(newCoord);

		//check to make sure not moveing more than the max distance
		if (moveDist>Constants.maxDistPerStep)moveDist=Constants.maxDistPerStep;

		//Perform move; if distance is outsite current foodsite
		//geog.moveByVector(this, moveDist, (angle));
		agent.getGeog().moveByDisplacement(agent, Math.cos(angle)*moveDist, Math.sin(angle)*moveDist);
		Resource newRes = SimUtils.findMyCurrentResource(agent, agent.getGeog());
		//agent.getGrid().moveTo(agent,newRes.getGridx(),newRes.getGridy());

		agent.updateMyPatch(newRes);
		agent.addNumbMoves();

	}

	//(2)
	public static void moveToNearestGroup(PrimateTopo agent){//used if primate considers it self not safe
		Coordinate group = GroupUtils.myNearestNeighs(agent);
		if (agent.getCoord().distance(group)>10){//myGroupsCenter()
			if (group!=null){
				moveTo(agent,group);//moves towards my nearest neighbours... number determined by desiered safe neigh size
			}else{
				System.out.println("error in model: move to nearest Group method, within the MoveUtils class!");
				//Coordinate random = new Coordinate(this.getCoord().x+RandomHelper.nextDoubleFromTo(-1, 1),this.getCoord().y+RandomHelper.nextDoubleFromTo(-1, 1));
				//moveTo(random);
			}
		}
	}
	
	public static void moveTowards (PrimateTopo p, Coordinate c){
		
		//calculate angle at which to move
		double adjasent = (double)(c.x-p.getCoord().x);
		double opposite = (double)(c.y-p.getCoord().y);
		double angle=0;

		angle = Math.atan(Math.abs(opposite/adjasent));
		if (adjasent<0) angle=(Math.PI)-angle;
		if (opposite<0) angle=2*(Math.PI)-angle;

		p.getGeog().moveByDisplacement(p, Math.cos(angle)*Constants.cellSize, Math.sin(angle)*Constants.cellSize);
		
	}
	
	public static void moveTowardsLeader(PrimateTopo p){
		//locate leader
		Coordinate c = null;
		for (PrimateTopo groupMate: p.getGroupMates()){
			if (groupMate.idNumb==0)c=groupMate.getCoord();
		}
		
		if (c == null){
			moveToNearestGroup(p);
		}else{
			moveTowards(p,c);
		}
		
	}
	
}
